/**
* @file        NaviEarthTools
* @brief       
* @version     1.0.0
* @author      Changlin.Jing 
* @date        2023/8/23 16:32
* @copyright   Copyright  2022 Langge Software Co., Ltd. All rights reserved.
*/
#ifndef LANGGE_VDR_NAVIEARTHTOOLS_HPP
#define LANGGE_VDR_NAVIEARTHTOOLS_HPP
#include "NaviConstant.h"
#include "3rd/eigen3/Eigen/Dense"

namespace UtilTools::Earth{
    using constant::constant_J2;
    using constant::constant_J4;
    using constant::constant_J6;
    using constant::WGS84_A;
    using constant::WGS84_AngleRate;
    using constant::WGS84_GM;
    using Eigen::Vector3d;

    template<typename T>
    inline T POW3(const T &t) {
        return t * t * t;
    }

    Vector3d CalculateGravity(const Vector3d &pos, bool IsECEF = true);

    /**
     * @brief  BLH coordinate convert to rectangle XYZ in WGS84 coordinate
     * @note
     * @param  &BLH: [rad rad m]
     * @retval
     */
    //Eigen::Vector3d WGS84BLH2XYZ(const Eigen::Vector3d &BLH);

    /**
     * @brief  XYZ coordinate convert to BLH in WGS84 coordinate
     * @note
     * @param  &XYZ: [m,m,m]
     * @retval
     */
    //Eigen::Vector3d WGS84XYZ2BLH(const Eigen::Vector3d &XYZ);

    /**
     * @brief  calculate the rotation matrix of ecef coordinate to navigation(local coordinate)
     * @note
     * @param  B:
     * @param  L:
     * @retval
     */
    Eigen::Matrix3d cne(const Vector3d &blh);

    //inline Eigen::Vector3d wiee() { return Eigen::Vector3d(0, 0, WGS84_AngleRate); }

    /**
     * @brief  Correct the leverarm for Position
     * @note
     * @param  &navinfo:
     * @retval
     */
//        inline Eigen::Vector3d CorrectLeverarmPos(){}

    /**
     * @brief  Correct the leverarm for Velocity
     * @note
     * @param  &navinfo:
     * @retval
     */
//        inline Eigen::Vector3d CorrectLeverarmVel() {}
    /**
  * @brief  n系(导航坐标系)到e系(地心地固坐标系)转换四元数
  * @param blh lat,long,height(rad,rad,meter)
  * @return
  */
    Eigen::Quaterniond blh2qne(const Eigen::Vector3d &blh);

    /**
 * @brief 从n系到e系转换四元数得到纬度和经度
 * @param qne
 * @param height
 * @return
 */
    Eigen::Vector3d qne2blh(const Eigen::Quaterniond &qne, double height);


    /***
     * @brief 计算子午圈半径和卯酉圈半径
     * @param lat
     * @return
     */
    Eigen::Vector2d meridianPrimeVerticalRadius(double lat);

    /**
     * @brief 计算卯酉圈半径
     * @param lat
     * @return
     */
    //double RN(double lat);

    /**
     * @brief n系相对位置转大地坐标相对位置
     * @param blh
     * @return
     */
    Eigen::Matrix3d DRi(const Eigen::Vector3d &blh);

    /***
     * @brief 计算大地相对位置转n系相对位置
     * @param blh
     * @return
     */
    Eigen::Matrix3d DR(const Vector3d &blh);
};


#endif //LANGGE_VDR_NAVIEARTHTOOLS_HPP
